/*
 * @FileName: agv_handler.cpp
 * @Author: weibin.yin
 * @Date: 2023-12-18 09:26:52
 * @Last Modified by: 
 * @Last Modified time: 
 */
#include "manual_handler.h"
#include <shared_parameters/shared_params_interface.hpp>

ManHandler::ManHandler() : Node("Agv_ManualHandler"),
  hd1000_active_(false),
  joy_active_(false),
  ioVelocityLinear_(0.0),
  ioHighVelocityLinear_(0.0),
  ioVelocityAngular_(0.0),
  ioHighVelocityAngular_(0.0),
  velFactor_(Factor_NONE)
{
  init_parameters();
  memset(&curve_, 0, sizeof(CurveObjectType));  
  
  zero_vel_.twist.linear.x = 0.0;  // 线速度（m/s）
  zero_vel_.twist.linear.y = 0.0;
  zero_vel_.twist.linear.z = 0.0;
  zero_vel_.twist.angular.x = 0.0;
  zero_vel_.twist.angular.y = 0.0;
  zero_vel_.twist.angular.z = 0.0; // 角速度（rad/s）
  
  init();
}

ManHandler::~ManHandler()
{
  quit_mutex_.unlock();
  if (thread_.joinable()) {
    thread_.join();
  }
}

void ManHandler::changeOpMode_Req(uint8_t mode)
{
  auto request = std::make_shared<agv_srvs::srv::SetOperationMode::Request>();

  std::string modeStr;
  switch (mode)
  {
    case agv_msgs::msg::HD1000::AUTO:
      modeStr= SET_MODE_AUTO;
      break;
    case agv_msgs::msg::HD1000::HALFAUTO:
      modeStr= SET_MODE_HALF_AUTO;
      break;
    case agv_msgs::msg::HD1000::MANUAL:
      modeStr= SET_MODE_MANUL;
      break;    
    default:
      break;
  }

  request->mode = modeStr;
  request->sender = 0;
  request->submode = "";
  
  auto result = operationModeClient_->async_send_request(request);
  
  // Wait for the result
  if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
      rclcpp::FutureReturnCode::SUCCESS)
  {
    DEBUG_WARN_OUT("HD1000 Set [" << modeStr  << " ] mode success!" );
  }
  else {
    DEBUG_ERROR_OUT("HD1000 Set [" << modeStr  << " ] mode failed");
  }
}

void ManHandler::callOutputSrv(std::string outStr,bool needOpen)
{
  auto request = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
  agv_msgs::msg::PairTypeInt out;

  out.key = outStr;
  out.value = needOpen;

  request->array.push_back(out);
  
  auto result = outputMapClient_->async_send_request(request);
  
  if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
      rclcpp::FutureReturnCode::SUCCESS)
  {
    DEBUG_OUT("Set output[" << outStr  << " ]:"<< needOpen << " success!" );
  }
  else {
    DEBUG_ERROR_OUT("Set output[" << outStr  << " ]:"<< needOpen << " failed!" );
  }
}

void ManHandler::joy_cb(const std_msgs::msg::UInt16MultiArray::SharedPtr multArr)
{
  agv_msgs::msg::HD1000  temp_state;
  if ( multArr->data.size() != 7 ) {
    DEBUG_ERROR_OUT("[HD1000_cb]wrong msg size:" << multArr->data.size());
    return;  
  }
  std::lock_guard<std::mutex> locker(mutex_);

  temp_state.valid= true;
  temp_state.angle= multArr->data[0];
  temp_state.speed= multArr->data[1];
  temp_state.buttons= multArr->data[6];
  temp_state.auto_mode= getBit( multArr->data[6], agv_msgs::msg::HD1000::AUTO_BIT);
  temp_state.halfauto_mode= getBit( multArr->data[6], agv_msgs::msg::HD1000::HALFAUTO_BIT);
  temp_state.manual_mode= getBit( multArr->data[6], agv_msgs::msg::HD1000::MANUAL_BIT);
  temp_state.forward= getBit( multArr->data[6], agv_msgs::msg::HD1000::FORWARD_BIT); 
  temp_state.low_move= getBit( multArr->data[6], agv_msgs::msg::HD1000::LOW_MOVE_BIT);       
  temp_state.high_move= getBit( multArr->data[6], agv_msgs::msg::HD1000::HIGH_MOVE_BIT);    
  temp_state.reset= getBit( multArr->data[6], agv_msgs::msg::HD1000::RESET_BIT); 
  temp_state.load1_up= getBit( multArr->data[6], agv_msgs::msg::HD1000::LOAD1_UP_BIT); 
  temp_state.load1_down= getBit( multArr->data[6], agv_msgs::msg::HD1000::LOAD1_DOWN_BIT); 
  temp_state.load2_left= getBit( multArr->data[6], agv_msgs::msg::HD1000::LOAD2_LEFT_BIT); 
  temp_state.load2_right= getBit( multArr->data[6], agv_msgs::msg::HD1000::LOAD2_RIGHT_BIT); 
  temp_state.e_stop= getBit( multArr->data[6], agv_msgs::msg::HD1000::ESTOP_BIT); 
  temp_state.backward= getBit( multArr->data[6], agv_msgs::msg::HD1000::BACKWARD_BIT); 
  temp_state.deadman= getBit( multArr->data[6], agv_msgs::msg::HD1000::DEADMAN_BIT); 

  if ( temp_state.angle > 15000 || temp_state.angle < -15000 ) {
    DEBUG_ERROR_OUT("[HD1000_cb]wrong angle value:" << temp_state.angle);
    return;  
  }
  if ( temp_state.speed > 100 || temp_state.speed < 0 ) {
    DEBUG_ERROR_OUT("[HD1000_cb]wrong speed value:" << temp_state.speed);
    return;  
  }
  if ( (temp_state.auto_mode && temp_state.halfauto_mode)
    || (temp_state.auto_mode && temp_state.manual_mode)
    || (temp_state.halfauto_mode && temp_state.manual_mode)
    || (temp_state.low_move && temp_state.high_move)) {
      DEBUG_ERROR_OUT("[HD1000_cb]wrong buttons value:" << temp_state.buttons);
    return;  
  }

  // nomal HD1000 msg
  if ( temp_state.auto_mode) 
    temp_state.operation_mode= agv_msgs::msg::HD1000::AUTO;
  else if ( temp_state.halfauto_mode)
    temp_state.operation_mode= agv_msgs::msg::HD1000::HALFAUTO;
  else
    temp_state.operation_mode= agv_msgs::msg::HD1000::MANUAL;

  temp_state.resultant_vel= curve_.currentSpeed;
  temp_state.acc= curve_.accRT;
  temp_state.jerk= curve_.jerk;
  manHandler_pub_->publish(temp_state);    
  hand_state_= temp_state;
  hd1000_active_.Update( true);
}

/* 电机曲线加减速操作-------------------------------------------------------- */
void ManHandler::resultantVelocityCurve(CurveObjectType *curve)
{
  static float last_speed(0.0f);
  static bool move_state(false);
/* 合速度产生-------------------------------------------------------- */
  
  float scale =   ( steer_.steering 
                  || steer_.chassis_not_ready ) 
                ? 0.0001f 
                : 0.01f;
  
  float sign = hand_state_.forward ? 1.0f : (-1.00f);

  bool reverse = reverseCheck( sign, curve_.currentSpeed);
  bool stop = ((event_.action == agv_msgs::msg::AGVEvent::ESTOP || event_.action == agv_msgs::msg::AGVEvent::QUICK_STOP || daemon_vel_.IsNew()) && !hand_state_.reset)
              || event_.ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT
              || steer_.steer_err
              || reverse;    
  bool move = ( hand_state_.low_move || hand_state_.high_move) && !stop;

  curve->targetSpeed = move \
                      ? hand_state_.speed * sign * scale
                      : 0.0f;
  if (hand_state_.angle > 9000 )  curve->targetAngle = 90.0f;
  else if (hand_state_.angle < -9000 )  curve->targetAngle = -90.0f;
  else  curve->targetAngle = hand_state_.angle * 0.01f; 
  
  float temp=0;
  float previous_v= curve->currentSpeed;
  float previous_acc= curve->accRT;

  curve->targetSpeed= std::min(curve->maxSpeed, std::max(curve->targetSpeed, curve->minSpeed));
  if(curve->targetSpeed != last_speed )
  {
    last_speed= curve->targetSpeed;
    curve->startSpeed= curve->currentSpeed; 
    curve->startSpeed= std::max(curve->startSpeed, curve->minSpeed);     
    temp= fabs(curve->targetSpeed - curve->startSpeed);
    temp= temp / curve->stepSpeed;
    curve->maxTimes= (uint32_t)(temp);
    curve->aTimes= 0;
  }
  
  /* S move-------------------------------------------------------- */  
  if(curve->aTimes < curve->maxTimes)
  {
     if ( curve->curveMode == CURVE_TRAP)
     {
        CalCurveTRAP(curve);
     }
     else if ( curve->curveMode == CURVE_SPTA)
     {
        CalCurveSPTA(curve);
     }
  
     curve->aTimes++;
     curve->accRT= (curve->currentSpeed - previous_v);
     curve->jerk= (curve->accRT - previous_acc);
  }
  else
  {
     curve->currentSpeed= curve->targetSpeed;
     curve->maxTimes= 0;
     curve->aTimes= 0;
  }

  if ( move != move_state){
    move_state= move;
    DEBUG_OUT("[HD1000]" << "bts_key:" << hand_state_.buttons
                          << "; speed:" << hand_state_.speed
                          << "; angle:" << hand_state_.angle
                          << "; stop:" << stop 
                          << "; e.action:" << event_.action 
                          << "; e.ecode:" << event_.ecode 
                          << "; steer_err:" << std::to_string(steer_.steer_err)
                          << "; steering:" << std::to_string(steer_.steering)
                          << "; chassis_not_ready:" << std::to_string(steer_.chassis_not_ready)
                          << "; reverse:" << reverse
                          << "; targetSpeed:" << curve->targetSpeed
                          << "; currentSpeed:" << curve->currentSpeed);
  }
}

/*S型曲线速度计算*/
void ManHandler::CalCurveSPTA(CurveObjectType *spta)
{
  float power= 0.0;
  float speed= 0.0;
  
  power=( 2 * ( (float)spta->aTimes) - ((float)spta->maxTimes)) / ((float)spta->maxTimes);
  power=( 0.0 - spta->flexible) * power;
  
  speed= 1 + expf(power);
  speed= (spta->targetSpeed - spta->startSpeed) / speed;
  spta->currentSpeed= speed + spta->startSpeed;
  
  if(spta->currentSpeed > spta->maxSpeed)
  {
     spta->currentSpeed= spta->maxSpeed;
  }
  
  if(spta->currentSpeed < spta->minSpeed)
  {
     spta->currentSpeed= spta->minSpeed;
  }
}
/*梯形曲线速度计算*/
void ManHandler::CalCurveTRAP(CurveObjectType *trap)
{
  float slope=0.0;
  
  slope=(trap->targetSpeed-trap->startSpeed)/ trap->maxTimes;
    
  trap->currentSpeed= trap->startSpeed + slope * trap->aTimes;
 
  if(trap->currentSpeed > trap->maxSpeed)
  {
    trap->currentSpeed= trap->maxSpeed;
  }
  
  if(trap->currentSpeed < trap->minSpeed)
  {
    trap->currentSpeed= trap->minSpeed;
  }

}

/* 合速度分解-------------------------------------------------------- */
void ManHandler::Velocitydecomposition(float resultant_vel, float angle)
{
  geometry_msgs::msg::TwistStamped  vel_base;
  memset(&vel_base, 0, sizeof(geometry_msgs::msg::TwistStamped));
  static bool trigged_vy(false),trigged_vz(false);
  static float last_angle;

  Approach( last_angle, angle, curve_.angle_ramp);
  double radian = last_angle * M_PI / 180.0;
  trigged_vy= (trigged_vy || hand_state_.low_move)      //自锁条件
            && (resultant_vel != 0.0f);     //复位条件
  trigged_vz= (trigged_vz || hand_state_.high_move)      //自锁条件
            && (resultant_vel != 0.0f);     //复位条件 

  vel_base.twist.linear.x= resultant_vel * cos(radian);
  vel_base.twist.linear.y= trigged_vy ? resultant_vel * sin(radian) : 0.0f;
  vel_base.twist.angular.z= trigged_vz ? resultant_vel * sin(radian) : 0.0f;
  
  if ( resultant_vel != 0.0 )
  {
    if ( (mode_.mode == agv_msgs::msg::AGVMode::MANUAL) && hand_state_.reset )
    {
      man_override_pub_->publish(vel_base); 
    }
    else if ( mode_.mode == agv_msgs::msg::AGVMode::MANUAL)
    {
      man_vel_pub_->publish(vel_base);
    }
    else if ( mode_.mode == agv_msgs::msg::AGVMode::SEMI_AUTOMATIC)
    {
      semi_vel_pub_->publish(vel_base);
    }    
  }
  
}

void ManHandler::agvmodeCallback(const agv_msgs::msg::AGVMode::SharedPtr state_p)
{
    mode_ = *state_p;
}

void ManHandler::steerCallback(const agv_msgs::msg::SteerDirFeedBack::SharedPtr state_p)
{
    steer_ = *state_p;
}

void ManHandler::odomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_p)
{
    odom_ = *odom_p;
}

void ManHandler::daemonCallback(const geometry_msgs::msg::TwistStamped::SharedPtr daemon_p)
{
    daemon_vel_.Update( *daemon_p);
    // DEBUG_WARN_OUT("[daemonCallback]vx:" << daemon_vel_.Get().linear.x  << ";vy:" << daemon_vel_.Get().linear.y << ";vz:" << daemon_vel_.Get().angular.z );
}

double ManHandler::get_max( geometry_msgs::msg::TwistStamped vel)
{
  double vel_main(0.0);
  double vx,vy,va;
  if ( !haveVel( vel))
    return vel_main;

  vx = std::abs(vel.twist.linear.x);
  vy = std::abs(vel.twist.linear.y);
  va = std::abs(vel.twist.angular.z);
  vel_main = vx > vy ? vel.twist.linear.x : vel.twist.linear.y;
  vel_main = va > std::abs(vel_main) ? vel.twist.angular.z : vel_main;
  return vel_main;
}

VelocityFactorType ManHandler::factorSelect(geometry_msgs::msg::TwistStamped twist)
{
  VelocityFactorType factor;
  if (twist.twist.linear.x != 0.0)
  {
    factor= Factor_VX;
  }
  else if (twist.twist.linear.y != 0.0)
  {
    factor= Factor_VY;
  }
  else if (twist.twist.angular.z != 0.0)
  {
    factor= Factor_VZ;
  }   
  else
    factor= Factor_NONE;
  return factor;
}
bool ManHandler::reverseCheck( geometry_msgs::msg::TwistStamped newVel, geometry_msgs::msg::TwistStamped lastVel)
{
  bool ret(false);
  VelocityFactorType directFactor;
  switch (velFactor_)
  {
  case Factor_NONE:
    directFactor= factorSelect(newVel);
    break;

  case Factor_VX:
    if (newVel.twist.linear.x != 0.0)
    {
      ret= reverseCheck(newVel.twist.linear.x, lastVel.twist.linear.x);
    }
    else
      directFactor= factorSelect(newVel);      
    break;

  case Factor_VY:
    if (newVel.twist.linear.y != 0.0)
    {
      ret= reverseCheck(newVel.twist.linear.y, lastVel.twist.linear.y);
    }
    else
      directFactor= factorSelect(newVel);   
    break;

  case Factor_VZ:
    if (newVel.twist.angular.z != 0.0)
    {
      ret= reverseCheck(newVel.twist.angular.z, lastVel.twist.angular.z);
    }
    else
      directFactor= factorSelect(newVel);   
    
    break;

  default:
    break;
  }
  ret = ret || (velFactor_ != directFactor);
  velFactor_ = directFactor;
  return ret;
}

bool ManHandler::reverseCheck( double max_new, double max_last)
{
  bool ret= (max_last > 0 && max_new < 0) 
         || (max_last < 0 && max_new > 0); 
  return ret;
}

void ManHandler::keyboardCallback(const geometry_msgs::msg::TwistStamped::SharedPtr pMsg)
{
    keyboard_vel_.Update(*pMsg);
}

void ManHandler::keyboardOverrideCallback(const geometry_msgs::msg::TwistStamped::SharedPtr pMsg)
{
    keyboardOverride_vel_.Update(*pMsg);
}

void ManHandler::event_Callback(const agv_msgs::msg::AGVEventStatus::SharedPtr event)
{
    event_= *event;
    
    auto ledState = std_msgs::msg::UInt8MultiArray();
    std_msgs::msg::UInt8 btnElement;
    btnElement.data = event->ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT;
    ledState.data.push_back( btnElement.data);
    handlerLED_pub_->publish(ledState);
}

void ManHandler::inputmap_Callback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg)
{
  static bool chassisMode_last;
  bool haveIO_joy(false);
  std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg->array;
  for (auto &data : inputs_msg){
      if (data.key == "driveManulAuto"){
          inputs_.chassisMode= (data.value == 1);
      }
      else if(data.key == "forwardMove"){
          inputs_.forwardMove= (data.value == 1);
          haveIO_joy = true;
      }
      else if(data.key == "backwardMove"){
          inputs_.backwardMove= (data.value == 1);
          haveIO_joy = true;
      }
      else if(data.key == "leftCrab"){
          inputs_.leftCrab= (data.value == 1);
          haveIO_joy = true;
      }
      else if(data.key == "rightCrab"){
          inputs_.rightCrab= (data.value == 1);
          haveIO_joy = true;
      }      
      else if(data.key == "leftTurn"){
          inputs_.leftTurn= (data.value == 1);
          haveIO_joy = true;
      }      
      else if(data.key == "rightTurn"){
          inputs_.rightTurn= (data.value == 1);
          haveIO_joy = true;
      } 
      else if(data.key == "lift"){
          inputs_.lift= (data.value == 1);
          
      }        
      else if(data.key == "fall"){
          inputs_.fall= (data.value == 1);
      }
      else if(data.key == "deepPressed"){
          inputs_.deepPressed= (data.value == 1);
      }
             
  }

  if (chassisMode_last != inputs_.chassisMode){
    chassisMode_last= inputs_.chassisMode;
    callOutputSrv( "driveControl", inputs_.chassisMode);
  }

  buttonVel( haveIO_joy);
}

void ManHandler::periphPub(void)
{
  static periphTeleopBut periph_ctl_last(NO_ACTIVE);
  periphTeleopBut periph_ctl = NO_ACTIVE;
  if ( hand_state_.load1_up || inputs_.lift){
    periph_ctl = UP;
  }
  else if (hand_state_.load1_down || inputs_.fall){
    periph_ctl = DOWN;
  }
  else if (hand_state_.load2_left)
  {
    periph_ctl = CLOCKWISE_ROTATION;
  }
  else if (hand_state_.load2_right)
  {
    periph_ctl = ANTICLOCKWISE_ROTAION;
  }  
  else
  {
    periph_ctl = NO_ACTIVE;
  }

  if ( (periph_ctl != periph_ctl_last)
    || (periph_ctl != NO_ACTIVE))
  {
    auto periphMsg = agv_msgs::msg::PeriphTeleop();
    periphMsg.freq = 30;
    periphMsg.order = periph_ctl;
    periphTeleop_pub_->publish(periphMsg);
    periph_ctl_last= periph_ctl;
  }
}

void ManHandler::bumperShiled()
{
  static bool last_hand_reset(false);
  if (hand_state_.reset != last_hand_reset){
    last_hand_reset= hand_state_.reset;
    callOutputSrv( "bumperShield",hand_state_.reset);
  }
}

void ManHandler::buttonVel( bool have)
{
  geometry_msgs::msg::TwistStamped btn_vel;
  double scale(0.0);

  if (mode_.mode != agv_msgs::msg::HD1000::MANUAL || !have)
    return;

  if (inputs_.forwardMove) scale = 1.0;
  else if (inputs_.backwardMove) scale = -1.0;
  else  scale = 0.0;
  btn_vel.twist.linear.x = inputs_.deepPressed ? (ioHighVelocityLinear_ * scale) : (ioVelocityLinear_ * scale);

  if (inputs_.leftCrab) scale = 1.0;
  else if (inputs_.rightCrab) scale = -1.0;
  else  scale = 0.0;
  btn_vel.twist.linear.y = inputs_.deepPressed ? (ioHighVelocityLinear_ * scale) : (ioVelocityLinear_ * scale);

  if (inputs_.leftTurn) scale = 1.0;
  else if (inputs_.rightTurn) scale = -1.0;
  else  scale = 0.0;
  btn_vel.twist.angular.z = inputs_.deepPressed ? (ioHighVelocityAngular_ * scale) : (ioVelocityAngular_ * scale);

  button_vel_.Update(btn_vel);
}

void ManHandler::speedLimitKeep( geometry_msgs::msg::TwistStamped &vel)
{
  float vz_max(0.0f),vz_min(0.0f);
  if (vel.twist.linear.x > 0.0 && vel.twist.linear.x  > curve_.maxSpeed)
    vel.twist.linear.x = curve_.maxSpeed;
  else if (vel.twist.linear.x < 0.0 && vel.twist.linear.x < curve_.minSpeed)
    vel.twist.linear.x = curve_.minSpeed;

  if (vel.twist.linear.y > 0.0 && vel.twist.linear.y > curve_.maxSpeed)
    vel.twist.linear.y = curve_.maxSpeed;
  else if (vel.twist.linear.y < 0.0 && vel.twist.linear.y < curve_.minSpeed)
    vel.twist.linear.y = curve_.minSpeed;

  if ( vmax_angular_ != 0.0){
    vz_max = vmax_angular_;
    vz_min = -vmax_angular_;
  }
  else
  {
    vz_max = curve_.maxSpeed;
    vz_min = curve_.minSpeed;
  }
  if (vel.twist.angular.z > 0.0 && vel.twist.angular.z > vz_max)
    vel.twist.angular.z = vz_max;
  else if (vel.twist.angular.z < 0.0 && vel.twist.angular.z < vz_min)
    vel.twist.angular.z = vz_min;
}

//keyboard vel
void ManHandler::keyboardVel(void)
{
  static geometry_msgs::msg::TwistStamped  vel_last;
  static bool trigged_normal(false),trigged_Override(false),stop(false);
  static bool move_state(false);
  geometry_msgs::msg::TwistStamped  vel_target;
  double step;
  
  if (mode_.mode == agv_msgs::msg::AGVMode::AUTOMATIC){
    trigged_normal= false;
    trigged_Override= false;
    stop= false;
    vel_last= zero_vel_;
    return;
  }

  if ( button_vel_.IsNew() && haveVel( button_vel_.Get())){
    vel_target = button_vel_.Get();
  }
  else if ( keyboard_vel_.IsNew() && haveVel( keyboard_vel_.Get()))
  {
    vel_target = keyboard_vel_.Get();
  }
  else if ( keyboardOverride_vel_.IsNew() && haveVel( keyboardOverride_vel_.Get()))
  {
    vel_target = keyboardOverride_vel_.Get();
  }
  else if ( !joy_active_.Get())   //直接退出，无需计算后续
  {
    vel_target = zero_vel_;
    inputs_.reset_signal();
    return;   //直接退出，无需计算后续
  }

  speedLimitKeep(vel_target);   //速度最大值限制

  bool reverse = reverseCheck( vel_target, vel_last);
  stop= (stop || ((event_.action == agv_msgs::msg::AGVEvent::ESTOP || event_.action == agv_msgs::msg::AGVEvent::QUICK_STOP || daemon_vel_.IsNew() ) && !keyboardOverride_vel_.IsNew())
              || event_.ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT
              || steer_.steer_err
              || reverse)          //自锁条件
      && ( joy_active_.Get());     //复位条件;    

  if ( stop) 
  {
    vel_target = zero_vel_;
  }

  if ( steer_.steering || steer_.chassis_not_ready)
  {
    double scale = std::abs( get_max( vel_target)) / 0.0001;
    vel_last.twist.linear.x = scale != 0 ? vel_target.twist.linear.x / scale : 0.0;
    vel_last.twist.linear.y = scale != 0 ? vel_target.twist.linear.y / scale : 0.0;
    vel_last.twist.angular.z = scale != 0 ? vel_target.twist.angular.z / scale : 0.0;

  }
  else
  {
    step= curve_.stepSpeed;
    Approach( vel_last.twist.linear.x, vel_target.twist.linear.x, step);
    Approach( vel_last.twist.linear.y, vel_target.twist.linear.y, step);
    Approach( vel_last.twist.angular.z, vel_target.twist.angular.z, acc_angular_);
  }

  if ( haveVel(vel_last))
    joy_active_.Update( true);

  trigged_normal= (trigged_normal || button_vel_.IsNew() || keyboard_vel_.IsNew())      //自锁条件
            && ( joy_active_.Get());     //复位条件
  trigged_Override= (trigged_Override || keyboardOverride_vel_.IsNew())      //自锁条件
            && ( joy_active_.Get());     //复位条件    

  if ( mode_.mode == agv_msgs::msg::AGVMode::MANUAL && trigged_Override)
  {
    man_override_pub_->publish(vel_last); 
  }
  else if ( mode_.mode == agv_msgs::msg::AGVMode::MANUAL && trigged_normal)
  {
    man_vel_pub_->publish(vel_last);
  }
  else if ( mode_.mode == agv_msgs::msg::AGVMode::SEMI_AUTOMATIC && (trigged_Override || trigged_normal))
  {
    semi_vel_pub_->publish(vel_last);
  } 

  if ( joy_active_.TimeOut())
  {
    joy_active_.Update( false);
  } 

  if ( joy_active_.Get() != move_state || stop){
    move_state= joy_active_.Get();
    DEBUG_OUT("[keyboardVel]" << "IO_btn:" << button_vel_.IsNew()
                              << "; keyboard:" << keyboard_vel_.IsNew()
                              << "; keyboardOverride:" << keyboardOverride_vel_.IsNew()
                              << "; stop:" << stop 
                              << "; e.action:" << event_.action 
                              << "; e.ecode:" << event_.ecode                                   
                              << "; steer_err:" << std::to_string(steer_.steer_err)
                              << "; steering:" << std::to_string(steer_.steering)
                              << "; not_ready:" << std::to_string(steer_.chassis_not_ready)
                              << "; reverse:" << reverse
                              << "; vx:" << vel_last.twist.linear.x
                              << "; vx:" << vel_last.twist.linear.x
                              << "; vz:" << vel_last.twist.angular.z);
  }
}

void ManHandler::handlerDevice(void)
{
  double elapsed_secs = 0.0;  
  if ( !hd1000_active_.Get()){
    hand_state_= null_state_;
    return;
  }

  std::lock_guard<std::mutex> locker(mutex_);
  if(mode_.mode != agv_msgs::msg::AGVMode::AUTOMATIC){
    if( bumperShield_ >= 0) bumperShiled();
    resultantVelocityCurve( &curve_);
    Velocitydecomposition( curve_.currentSpeed, curve_.targetAngle);
  }
  if (hand_state_.operation_mode != mode_.mode ){
    changeOpMode_Req( hand_state_.operation_mode);
  }
  //键盘速度超时
  if ( hd1000_active_.TimeOut())
  {
    hd1000_active_.Update(false);
  }
}

void ManHandler::thread_fun(ManHandler *pThis)
{
  if (!pThis)
    return;

  while( !pThis->quit_mutex_.try_lock_for(std::chrono::milliseconds( 10))){
    pThis->handlerDevice();
    pThis->keyboardVel();
    pThis->periphPub();
  }
}

bool ManHandler::init_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    DEBUG_ERROR_OUT( "Failed to initialize shared memory interface");
    return false;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      DEBUG_ERROR_OUT("Error get shared parameters");
      return false;
    } 

    params = SharedParamsInterface::getNodeParams("manualDevice");
    for (const auto& [name, value] : params) {
      DEBUG_OUT("get param:" << name << ", value:" << value.toString());
      switch(value.type) {
        case SharedParamsInterface::ParamValue::Type::INT:
            this->declare_parameter(name, value.getValue<int>());
            break;
        case SharedParamsInterface::ParamValue::Type::DOUBLE:
            this->declare_parameter(name, value.getValue<double>());
            break;
        case SharedParamsInterface::ParamValue::Type::STRING:
            this->declare_parameter(name, value.getValue<std::string>());
            break;
        case SharedParamsInterface::ParamValue::Type::BOOL:
            this->declare_parameter(name, value.getValue<bool>());
            break;
        default:
            DEBUG_WARN_OUT("Unsupported parameter type for " << name);
            break;
      }
    }

    params = SharedParamsInterface::getNodeParams("IO");
    for (const auto& [name, value] : params) {
      if (name.find("output_device/driveControl") != std::string::npos || 
        name.find("output_device/bumperShield") != std::string::npos) {
        DEBUG_OUT("get param :" << name << ", value:" << value.toString());
        switch(value.type) {
          case SharedParamsInterface::ParamValue::Type::INT:
              this->declare_parameter(name, value.getValue<int>());
              break;
          case SharedParamsInterface::ParamValue::Type::DOUBLE:
              this->declare_parameter(name, value.getValue<double>());
              break;
          case SharedParamsInterface::ParamValue::Type::STRING:
              this->declare_parameter(name, value.getValue<std::string>());
              break;
          case SharedParamsInterface::ParamValue::Type::BOOL:
              this->declare_parameter(name, value.getValue<bool>());
              break;
          default:
              DEBUG_WARN_OUT("Unsupported parameter type for " << name);
              break;
        }
      }
    }
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Error reading parameters: " << e.what());
    return false;
  }
  return true;
}

/*!
 * \brief Initializes node to get parameters, subscribe to topics.
 */
void ManHandler::init()
{
  // common
  readParam("driveControl", driveControl_, -1);
  readParam("bumperShield", bumperShield_, -1);
  int curveMode;
  readParam("curveType", curveMode, 1);
  curve_.curveMode= (SpeedCurveType)curveMode;
  readParam("stepSpeed", curve_.stepSpeed, 0.008f);
  readParam("maxSpeed", curve_.maxSpeed, 1.0f);
  readParam("minSpeed", curve_.minSpeed, -1.0f);
  readParam("vmax_angular", vmax_angular_, 0.0f);
  readParam("acc_angular", acc_angular_, (double)(curve_.stepSpeed));
  readParam("flexible", curve_.flexible, 5.2f);
  readParam("angleRamp", curve_.angle_ramp, 2.2f);
  readParam("ioVelocity", ioVelocityLinear_, 0.05);
  if ( ioVelocityLinear_ > 1.0){
    ioVelocityLinear_= 1.0;
    DEBUG_WARN_OUT("ioVelocity too high:up limit"<< ioVelocityLinear_);
  }
  readParam("ioVelocity_angular", ioVelocityAngular_, 0.05);
  if ( ioVelocityAngular_ > 1.0){
    ioVelocityAngular_= 1.0;
    DEBUG_WARN_OUT("ioVelocity_angular too high:up limit"<< ioVelocityAngular_);
  }
  readParam("ioHighVelocity", ioHighVelocityLinear_, 0.1);
  if ( ioHighVelocityLinear_ > 1.0){
    ioHighVelocityLinear_= 1.0;
    DEBUG_WARN_OUT("ioVelocity too high:up limit"<< ioHighVelocityLinear_);
  }
  readParam("ioHighVelocity_angular", ioHighVelocityAngular_, 0.05);
  if ( ioHighVelocityAngular_ > 1.0){
    ioHighVelocityAngular_= 1.0;
    DEBUG_WARN_OUT("ioVelocity_angular too high:up limit"<< ioHighVelocityAngular_);
  }
  
  // Create subscribers
  joy_sub_ = this->create_subscription<std_msgs::msg::UInt16MultiArray>(
    "/HD1000/CMD0001", 1, std::bind(&ManHandler::joy_cb, this, std::placeholders::_1));
  
  agvmode_sub_ = this->create_subscription<agv_msgs::msg::AGVMode>(
    "/agvmode", 1, std::bind(&ManHandler::agvmodeCallback, this, std::placeholders::_1));
  
  eventOccurSub_ = this->create_subscription<agv_msgs::msg::AGVEventStatus>(
    "/eventstatus", 1, std::bind(&ManHandler::event_Callback, this, std::placeholders::_1));
  
  steerSub_ = this->create_subscription<agv_msgs::msg::SteerDirFeedBack>(
    "/steer_state", 1, std::bind(&ManHandler::steerCallback, this, std::placeholders::_1));
  
  odomSub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
    "/odom", 1, std::bind(&ManHandler::odomCallback, this, std::placeholders::_1));
  
  daemonSub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
    "/twist_mux/daemon_vel", 1, std::bind(&ManHandler::daemonCallback, this, std::placeholders::_1));
  
  handlerEnableSub_ = this->create_subscription<agv_msgs::msg::ReadInPutsNew>(
    "/readInputNew", 1, std::bind(&ManHandler::inputmap_Callback, this, std::placeholders::_1));
  
  keyboardSub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
    "/twist_mux/keyboard_vel", 1, std::bind(&ManHandler::keyboardCallback, this, std::placeholders::_1));
  
  keyboardOverrideSub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
    "/twist_mux/keyboard_override_vel", 1, std::bind(&ManHandler::keyboardOverrideCallback, this, std::placeholders::_1));
  
  joyOverrideSub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
    "/twist_mux/command_teleop_override_joy", 1, std::bind(&ManHandler::keyboardOverrideCallback, this, std::placeholders::_1));

  // Create service clients
  operationModeClient_ = this->create_client<agv_srvs::srv::SetOperationMode>("/operation_mode");
  outputMapClient_ = this->create_client<agv_srvs::srv::OutputsNew>("/outputNew");

  // Create publishers
  man_vel_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/twist_mux/manual_vel", 1);
  man_override_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/twist_mux/manual_override_vel", 1);
  semi_vel_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/semi_auto_vel", 1);
  semi_override_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/semi_auto_override_vel", 1);
  periphTeleop_pub_ = this->create_publisher<agv_msgs::msg::PeriphTeleop>("/base/periphTeleopInfo", 1);
  manHandler_pub_ = this->create_publisher<agv_msgs::msg::HD1000>("/manualDevice", 1);
  fork_lift_pub_ = this->create_publisher<std_msgs::msg::Float64>("/base/controllers/forkLift_controller/command", 1);
  handlerLED_pub_ = this->create_publisher<std_msgs::msg::UInt8MultiArray>("/HD1000/LEDState", 1);

  quit_mutex_.lock();
  thread_ = std::thread(thread_fun, this);
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ManHandler>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}